#include <math.h>
#include "FOC.h"
#include "FOC_conf.h"
#include "FOC_utils.h"
#include "stm32_mcu.h"
#include "AS5600.h"
#include "stdio.h"
#include "FOC_LPF.h"
#include "time_utils.h"
#include "stm32f1xx_it.h"
#include "FOC_PID.h"

FOC_CONTROL_MODE mode = OPEN_LOOP_POSITION_CONTROL;
ControlMode_t ctrlMode = ANGLE;
float target_velocity = 0;
float target_angle = 0;
struct DQVoltage_s voltage;
float zero_electrical_angle = 5.6280;
float zero_electrical_offset = 5.6280;//5.54209   5.58505 5.69245

int8_t sensorDirection = 1; //
float electricalAngle;
float estAngle;
float estVelocity;
struct_FOC_t st_FOC={
        .Ts = 0.001,
        .Pn = 7,
        .Kv = 190,
        .R = 10,
        .voltageUsedForSensorAlign = 1.5f,
        .PowerVoltage = 12,
        .LimitVoltage = 1,
        .LimitVelocity = 15,
        .LimitCurrent = 0.15,
};

void FOC_init() {
  AS5600_Init();
  _init3PWM();
  //alignSensor();
  FOC_lpf_set_Parameters();
  pid_set_parameter();
  if (ctrlMode == ANGLE){
    target = AS5600_getRawAngle();
  }
}


uint32_t g_magneticInterval=0;
//CW
void magneticTest(uint16_t tick){
  if(g_SysTick - g_magneticInterval < (UINT32_MAX >> 1)) {
    printf("%.2f\r\n", AS5600_getRawAngle());
    g_magneticInterval = g_SysTick + tick;
  }
}

float get_mechanical_angle() {
  float angle = sensorDirection * AS5600_getFullAngle();
  return angle;
}

float get_electrical_angle() {
  return _normalizeAngle(sensorDirection * st_FOC.Pn * AS5600_getLapAngle() - zero_electrical_offset );
}
float get_estVelocity(void){
  float estVel = sensorDirection * getVelocity();
  estVel = low_pass_filter(&lpf_velocity,estVel);
  printf("%.3f\r\n",estVel);
  return estVel;
}
//Encoder alignment to electrical 0 angle
void alignSensor(void){
  float angle;
  int err;
  // Align the electrical phases of the motor and sensor
  // Set angle -90(270 = 3PI/2) degrees
  printf("Align sensor\r\n");
  for (int i = 0; i <= 500; i++) {
    angle = _3PI_2 + _2PI * i / 500.0f;
    setPhaseVoltage(st_FOC.voltageUsedForSensorAlign, 0, angle);
    delay_ms(2);
  }
  angleUpdate();
  float midAngle = AS5600_getFullAngle();
  for (int i = 500; i >= 0; i--) {
    angle = _3PI_2 + _2PI * i / 500.0f;
    setPhaseVoltage(st_FOC.voltageUsedForSensorAlign, 0, angle);
    delay_ms(2);
  }
  angleUpdate();
  float endAngle = AS5600_getFullAngle();
  setPhaseVoltage(0,0,0);
  delay_ms(200);
  if (midAngle == endAngle){
    err = FAILED_TO_NOTICE_MOVEMENT;
    return;
  } else if (midAngle < endAngle){
    sensorDirection = CCW;
  } else{
    sensorDirection = CW;
  }
  printf("sensor direction:%d\r\n",sensorDirection);


  if (!_isset(zero_electrical_offset)){
    setPhaseVoltage(st_FOC.voltageUsedForSensorAlign, 0, _3PI_2);
    delay_ms(1000);
    angleUpdate();
    zero_electrical_offset = 0;
    zero_electrical_offset = get_electrical_angle();
    printf("zero_offset:%.4f\r\n",zero_electrical_offset);
    setPhaseVoltage(0,0,0);
    delay_ms(200);
  }
}

float angle=0;
uint32_t openLoopTimeStamp;
void velocityOpenLoop(float target_velocity){
  uint32_t now_us;
  float Ts;
  now_us = DWT_CYCCNT;
  Ts = (float)(DWT_CYCCNT - openLoopTimeStamp)/72 * 1e-6;
  angle = _normalizeAngle(angle + target_velocity * Ts);
  //use voltage limit or current limit
  float Uq = _isset(st_FOC.R)?st_FOC.LimitCurrent*st_FOC.R:st_FOC.LimitVoltage;
  setPhaseVoltage(Uq,0, _electricalAngle(angle,st_FOC.Pn));
  openLoopTimeStamp = now_us;
}

float open_pos = 0;
void angleOpenLoop(float target_angle){
  uint32_t now_us;
  float Ts;
  now_us = DWT_CYCCNT;
  Ts = (float)(DWT_CYCCNT - openLoopTimeStamp)/72 * 1e-6;

  if (fabs(target_angle - open_pos) > fabs(st_FOC.LimitVelocity * Ts)){
    open_pos += _sign(target_angle - open_pos)* fabs(st_FOC.LimitVelocity) * Ts;
  } else{
    open_pos = target_angle;
  }
  setPhaseVoltage(st_FOC.LimitVoltage,0, \
                  _electricalAngle(_normalizeAngle(open_pos), st_FOC.Pn));
 openLoopTimeStamp = now_us;
}

float setPointVelocity;
float setPointAngle;
void move(float new_target)
{
  if (ctrlMode != ANGLE_OPEN_LOOP && ctrlMode != VELOCITY_OPEN_LOOP){
    estAngle =get_mechanical_angle();
  }
  estVelocity = get_estVelocity();

  switch (ctrlMode) {
    case TORQUE:
      voltage.q = new_target;
      break;
    case VELOCITY:
      setPointVelocity = target;
      voltage.q = pid_calc(&pid_velocity,setPointVelocity - estVelocity);
      break;
    case ANGLE:
      setPointAngle = new_target;
      setPointVelocity = pid_calc(&pid_angle,setPointAngle - estAngle);
      voltage.q = pid_calc(&pid_velocity,setPointVelocity - estVelocity);
      break;
    case VELOCITY_OPEN_LOOP:
      velocityOpenLoop(new_target);
      break;
    case ANGLE_OPEN_LOOP:
      angleOpenLoop(new_target);
      break;
  }
}
void loopFoc(void)
{
  angleUpdate();
  if (ctrlMode == VELOCITY_OPEN_LOOP || ctrlMode == ANGLE_OPEN_LOOP)
    return;
  printf("q:%.3f\r\n",voltage.q);
  electricalAngle = get_electrical_angle();
  setPhaseVoltage(voltage.q,voltage.d,electricalAngle);
}
void setPhaseVoltage(float Uq, float Ud, float angle_el) {
  float Uout;
  if (Ud!=0){
    Uout=_sqrt(Ud*Ud+Uq*Uq)/st_FOC.PowerVoltage;   ///LimitVoltage/PowerVoltage  max:PowerVoltage
    angle_el= _normalizeAngle(angle_el+ atan2(Uq,Ud));
  } else{
    Uout=Uq/st_FOC.PowerVoltage;
    angle_el= _normalizeAngle(angle_el+_PI_2);
  }
  int sector = floor(angle_el / _PI_3) + 1;
  float T1 = _SQRT3 * _sin(sector * _PI_3 - angle_el) * Uout;
  float T2 = _SQRT3 * _sin(angle_el - (sector - 1.0f) * _PI_3) * Uout;
  float T0 = 1 - T1 - T2;
  float Ta, Tb, Tc;
  switch (sector) {
    case 1:
      Ta = T1 + T2 + T0 / 2;
      Tb = T2 + T0 / 2;
      Tc = T0 / 2;
      break;
    case 2:
      Ta = T1 + T0 / 2;
      Tb = T1 + T2 + T0 / 2;
      Tc = T0 / 2;
      break;
    case 3:
      Ta = T0 / 2;
      Tb = T1 + T2 + T0 / 2;
      Tc = T2 + T0 / 2;
      break;
    case 4:
      Ta = T0 / 2;
      Tb = T1 + T0 / 2;
      Tc = T1 + T2 + T0 / 2;
      break;
    case 5:
      Ta = T2 + T0 / 2;
      Tb = T0 / 2;
      Tc = T1 + T2 + T0 / 2;
      break;
    case 6:
      Ta = T1 + T2 + T0 / 2;
      Tb = T0 / 2;
      Tc = T1 + T0 / 2;
      break;
    default:
      // possible error state
      Ta = 0;
      Tb = 0;
      Tc = 0;
  }
  // Ta, Tb, Tc range [0,1]
  _writeDutyCycle3PWM(Ta, Tb, Tc);
}
